#define NDEBUG

#include "physics.h"
#include "3d.h"

#ifdef __physics_ext_bulletH__

using namespace WtfEngine;


MeshInterface::MeshInterface(const WtfGraphics::Mesh::tRef& rMesh)
	: btStridingMeshInterface(), mrMesh(rMesh), mIndex(0, 1, 2) {

};

void MeshInterface::getLockedVertexIndexBase(
		unsigned char **vertexbase,
		int& numverts, PHY_ScalarType& type, int& stride,
		unsigned char **indexbase,int & indexstride,int& numfaces,
		PHY_ScalarType& indicestype,int subpart)
{
	*vertexbase = (unsigned char *)mrMesh->getTriangles()[subpart][0].toArray();
	numverts = 9;
	type = PHY_DOUBLE;
	stride = sizeof(tScalar);
	*indexbase = (unsigned char *)mIndex.toArray();
	indexstride = sizeof(int);
	indicestype = PHY_INTEGER;
};

void MeshInterface::getLockedReadOnlyVertexIndexBase(
		const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,
		const unsigned char **indexbase,int & indexstride,int& numfaces,
		PHY_ScalarType& indicestype,int subpart) const
{
	*vertexbase = (unsigned char *)mrMesh->getTriangles()[subpart][0].toArray();
	numverts = 9;
	type = PHY_DOUBLE;
	stride = sizeof(tScalar);
	*indexbase = (unsigned char *)mIndex.toArray();
	indexstride = sizeof(int);
	indicestype = PHY_INTEGER;
};

void MeshInterface::unLockVertexBase(int subpart) {

};

void MeshInterface::unLockReadOnlyVertexBase(int subpart) const{

};

int MeshInterface::getNumSubParts() const {
	return mrMesh->getNumTris();
};

void MeshInterface::preallocateVertices(int numverts) {
	// TODO???
};

void MeshInterface::preallocateIndices(int numindices) {
	// Basically, each triangle stores its own vertices, so
	// all the indices form the sequence 0, 1, 2, 0, 1, 2...

};



PhysicalModel::PhysicalModel(const StringPool::Entry& sName, btCollisionShape * sh, tScalar mq)
	: DataObject(sName), mpCollisionShape(sh), mMassQuot(mq)
{
	if(mq != 0) sh->calculateLocalInertia(1.0 / mq, mLocalInertia);
};

PhysicalModel::~PhysicalModel() {
	delete mpCollisionShape;
};



/**** World Object ****/

WorldObject::WorldObject(const tCreationOptions& args, const WtfGraphics::GraphicsData::tCacheRef& d,
						 const PhysicalModel::tCacheRef& m,
						 const btRigidBody::btRigidBodyConstructionInfo& info)
: Renderable(args, d), mpBody(new btRigidBody(info)), mModel(&*m), mrModel(m)
{
	getBody()->setMotionState(&mState);
	getBody()->setUserPointer(this);
	PhysicsEngine::GetInstance().PushObject(this);
};

WorldObject::~WorldObject() {
	delete mpBody;
};

void WorldObject::RenderStart() {
	btTransform t;
	btScalar m[16];

	mState.getWorldTransform(t);
	t.getOpenGLMatrix(m);
	glPushMatrix();
	glMultMatrixf(m);
};

void WorldObject::Render() {
	RenderStart();
	RenderLocal();
	RenderEnd();
};

void WorldObject::RenderLocal() {
	Renderable::Render();
};

void WorldObject::RenderEnd() {
	glPopMatrix();
};

void WorldObject::Destroy() {
	Renderable::Destroy();
	PhysicsEngine::GetInstance().RemoveObject(this);
};


/// State of a serialized WorldObject
/// @note The cache reference to the physical model
///		  is given afterwards.
struct WorldObjectState {
	Transformation3D WorldTransform;
};

unsigned WorldObject::CalculateSerializedSize() {
	return Renderable::CalculateSerializedSize() + sizeof(WorldObjectState)
		+ mrModel.CalculateSerializedSize();
};

unsigned WorldObject::Serialize(void * pBuf) {
	unsigned n = Renderable::Serialize(pBuf);
	
	WorldObjectState * pState = reinterpret_cast<WorldObjectState *>((unsigned)pBuf + n);
	pState->WorldTransform = Transformation3D(getPosition(), getOrientation());
	n += sizeof(WorldObjectState);

	return n + mrModel.Serialize((void *)((unsigned)pBuf + n));
};

unsigned WorldObject::Deserialize(void * pBuf) {
	unsigned n = Renderable::Deserialize(pBuf);
	
	WorldObjectState * pState = reinterpret_cast<WorldObjectState *>((unsigned)pBuf + n);
	setPosition(pState->WorldTransform.getTranslation());
	setOrientation(pState->WorldTransform.getOrientation());
	n += sizeof(WorldObjectState);

	return n + mrModel.Deserialize((void *)((unsigned)pBuf + n));
};


/**** Physical ****/

Physical::Physical(const Physical::tCreationOptions& args,
				   const WtfGraphics::GraphicsData::tCacheRef& d,
		 const PhysicalModel::tCacheRef& m, bool isParticle)
	: WorldObject(args, d, m, btRigidBody::btRigidBodyConstructionInfo
			(1 / m->getMassQuot(), NULL, m->getCollisionShape(), m->getLocalInertia()))
{
	getBody()->setCollisionFlags(getBody()->getCollisionFlags() |
		btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
};

Physical::~Physical() {
	std::cout << "Physical destroyed\n";
};

struct PhysicalObjectState {
	tVector vVelocity;
	tVector vAngularVelocity;
};

unsigned Physical::CalculateSerializedSize() {
	return WorldObject::CalculateSerializedSize() + sizeof(PhysicalObjectState);
};

unsigned Physical::Serialize(void * pBuf) {
	unsigned n = WorldObject::Serialize(pBuf);
	
	PhysicalObjectState * pState = reinterpret_cast<PhysicalObjectState *>((unsigned)pBuf + n);
	pState->vVelocity = getVelocity();
	pState->vAngularVelocity = getAngularVelocity();
	n += sizeof(PhysicalObjectState);

	return n;
};

unsigned Physical::Deserialize(void * pBuf) {
	unsigned n = WorldObject::Deserialize(pBuf);

	PhysicalObjectState * pState = reinterpret_cast<PhysicalObjectState *>((unsigned)pBuf + n);
	setVelocity(pState->vVelocity);
	setAngularVelocity(pState->vAngularVelocity);
	n += sizeof(PhysicalObjectState);

	return n;
};

GameObject * Physical::CreateObject(const StringPool::Entry& sName) {
	tCreationOptions opts;
	opts.sName = sName;
	return new Physical(opts, WtfGraphics::GraphicsData::tCacheRef(), PhysicalModel::tCacheRef());
};


/**** Static ****/

Static::Static(const Static::tCreationOptions& args,
			   const WtfGraphics::GraphicsData::tCacheRef& d,
			   const PhysicalModel::tCacheRef& m)
	: WorldObject(args, d, m, btRigidBody::btRigidBodyConstructionInfo
			(0, NULL, m->getCollisionShape()))
{
};

Static::~Static() {

};

GameObject * Static::CreateObject(const StringPool::Entry& sName) {
	tCreationOptions opts;
	opts.sName = sName;
	return new Static(opts, WtfGraphics::GraphicsData::tCacheRef(), PhysicalModel::tCacheRef());
};


/**** Constraints ****/

Constraint::Constraint() : GcObject() {

};

Constraint::~Constraint() {
	std::cout << "Constraint destroyed\n";
};

PointConstraint::PointConstraint(const WorldObject::tRef& o1, const WorldObject::tRef& o2,
				  const tPosition& p1, const tPosition& p2):
		btPoint2PointConstraint(*o1->getBody(), *o2->getBody(),
				FROMVECT(p1), FROMVECT(p2)), Constraint()
{
	PhysicsEngine::GetInstance().AddConstraint(this, this);
};


btTransform __CalculateSliderTransform(const WorldObject::tRef& obj, const tVector& vPos, const tVector& vMin, const tVector& vMax) {
	Orientation3D ori;
	btTransform t;

	// Bullet slider constraint initially works on the X - axis, check if we
	// want another axis and set the orientation appropriately.
	if(vMax.normalize() != tVector(1,0,0)) {
		// Use householder reflection to rotate x-axis to desired axis.
		ori = (tVector(1,0,0) - vMax.normalize()).householder();
	};

	tScalar m[16];
	Transformation3D(vPos, ori * obj->getOrientation()).getOpenGLMatrix(m);
	t.setFromOpenGLMatrix(m);
	return t;
};

SliderConstraint::SliderConstraint(const WorldObject::tRef& o1, const WorldObject::tRef& o2,
				  const tPosition& p1, const tPosition& p2, const tVector& minLinear, const tVector& maxLinear)
: btSliderConstraint(*o1->getBody(), *o2->getBody(), __CalculateSliderTransform(o1,p1, minLinear, maxLinear),
							 __CalculateSliderTransform(o2,p2, minLinear, maxLinear), true), Constraint()
{
	this->setLowerLinLimit(-minLinear.length());
	this->setUpperLinLimit(maxLinear.length());
	this->setLowerAngLimit(-SIMD_PI);
	this->setUpperAngLimit(SIMD_PI);
	this->setDampingDirLin(0.05);
	this->setDampingDirAng(0.05);
	this->setRestitutionDirLin(1.0);
	PhysicsEngine::GetInstance().AddConstraint(this, this);
};


/**** Physics Engine (task) ****/

#define CAST_TO_WORLDOBJ(p)	const_cast<WorldObject *>(static_cast<const WorldObject *>((p)->getUserPointer()))

WorldObject::tRef PhysicsEngine::RayCast(const tRay& ray, tPosition& vHitPos) {
	btCollisionWorld::ClosestRayResultCallback result(FROMVECT(ray.vPosition), FROMVECT((ray.vDirection * 10000) + ray.vPosition));

	pWorld->rayTest(FROMVECT(ray.vPosition), FROMVECT((ray.vDirection * 10000) + ray.vPosition), result);

	if(result.hasHit()) {
		vHitPos = TOVECT(result.m_hitPointWorld);
		return CAST_TO_WORLDOBJ(result.m_collisionObject);
	} else {
		return WorldObject::tRef();
	};
};


ContactAddedCallback gOldContactAddedCallback;

bool myContactAddedCallback( btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,
                              int index0,const btCollisionObject* colObj1,int partId1,int index1 ) {
  
	WorldObject * pObj0 = CAST_TO_WORLDOBJ(colObj0);
	WorldObject * pObj1 = CAST_TO_WORLDOBJ(colObj1);

	//
	// Dispatch collision event, and only call Bullet's handler if the event is still propagating.
	//

	if(Kernel::GetInstance().ProcessEvent(
		new ObjectCollisionEvent(pObj0, pObj1)
	) == Event::PROPAGATING) {
		//return gOldContactAddedCallback(cp, colObj0, partId0, index0, colObj1, partId1, index1);
	} else {
		cp.m_lifeTime = 0;
	};

	return false;
};

#ifndef NDEBUG
#	include <bullet/Demos/OpenGL/GLDebugDrawer.h>
#endif

PhysicsEngine::PhysicsEngine():
Task<PhysicsEngine>(501, "Physics Engine") {

	btCollisionConfiguration *pCC = new btDefaultCollisionConfiguration();

	this->pDispatcher = new btCollisionDispatcher(pCC);
	
	// World AABB
	btVector3 worldAabbMin(-10000,-10000,-10000);
	btVector3 worldAabbMax(10000,10000,10000);
	int	maxProxies = 1024;
	this->pBroadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);

	this->pWorld = new btDiscreteDynamicsWorld(this->pDispatcher, this->pBroadphase,
		new btSequentialImpulseConstraintSolverPrefered(), pCC);

	this->pWorld->setGravity(btVector3(0, -90.8, 0));

	gOldContactAddedCallback = gContactAddedCallback;
	gContactAddedCallback = &myContactAddedCallback;

#ifndef NDEBUG
	this->pWorld->setDebugDrawer(new GLDebugDrawer());
	this->pWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawContactPoints | btIDebugDraw::DBG_DrawWireframe);
#endif
};

PhysicsEngine::~PhysicsEngine() {

};

void PhysicsEngine::Run() {
	this->pWorld->stepSimulation(Kernel::GetInstance().GetGameState()->GetDelta(), 10, 1.0/120.0);
};

#endif
